﻿using System;

namespace Tms.Biz
{
    public class WaveFilter
    {
        #region 20200624 ori  //注意：浮点型的滤波函数，实际上是个假的滤波函数。

        //低切滤波--频率表
        public string[] m_strLCFValue = new string[10] { "DC", "1Hz", "2Hz", "5Hz", "10Hz", "20Hz", "30Hz", "50Hz", "100Hz", "200Hz" };

        //低切滤波--系数（对应：DC、 1、2、5、10、20、30、50、100、200）
        private Int32[,] m_iLCFCofA = new Int32[10, 3] {
            {100000, 0, 0 },{99922, -99922, 0 }, { 99843, -99843, 0 },
            {99609, -99609, 0 }, { 99221, -99221, 0 },{ 98453, -98453, 0 },
            {96798, -96798, 0}, {96220, -96220,0}, {92704, -92704,0 }, {86327, -86327,0 } };

        private Int32[,] m_iLCFCofB = new Int32[10, 3] {
            {100000, 0, 0}, {100000, -99843, 0 }, {100000, -99685, 0 },
            {100000, -99218, 0 }, {100000, -98441, 0 },{100000, -96907, 0 },
            {100000, -95395, 0}, {100000, -92438, 0 }, {100000, -85407,0 }, {100000, -72654,0 } };

        //低切滤波--参数（外部选择的频率索引）
        public int m_iLCFSelected = 0;
        public string m_strLCFSelected
        {
            get
            {
                return m_strLCFValue[m_iLCFSelected];
            }
            set
            {
                if(int.TryParse(value, out int tmp))
                {
                    value += "Hz";
                }
                for (int i = 0; i < m_strLCFValue.Length; i++)
                {
                    if (value.ToLower() == m_strLCFValue[i].ToLower())
                    {
                        m_iLCFSelected = i;
                        return;
                    }
                }
            }
        }

        //低切滤波--参数（内部用）
        private long[] m_iLCFDataXs1 = new long[3];
        private long[] m_iLCFDataYs1 = new long[3];
        private long[] m_iLCFDataXs2 = new long[3];
        private long[] m_iLCFDataYs2 = new long[3];

        /// <summary>
        /// 通道1--低切滤波函数（浮点型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public float LowCutFilterCh1(float fdatain)
        {
            return fdatain;
        }

        /// <summary>
        /// 通道1--低切滤波函数（长整型）
        /// </summary>
        /// <param name="idatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public long LowCutFilterCh1(long idatain)
        {
            m_iLCFDataXs1[2] = m_iLCFDataXs1[1];
            m_iLCFDataXs1[1] = m_iLCFDataXs1[0];
            m_iLCFDataXs1[0] = idatain;

            m_iLCFDataYs1[2] = m_iLCFDataYs1[1];
            m_iLCFDataYs1[1] = m_iLCFDataYs1[0];
            m_iLCFDataYs1[0] = (m_iLCFDataXs1[0] * m_iLCFCofA[this.m_iLCFSelected, 0] + m_iLCFDataXs1[1] * m_iLCFCofA[this.m_iLCFSelected, 1] + m_iLCFDataXs1[2] * m_iLCFCofA[this.m_iLCFSelected, 2]
                - m_iLCFDataYs1[1] * m_iLCFCofB[this.m_iLCFSelected, 1] - m_iLCFDataYs1[2] * m_iLCFCofB[this.m_iLCFSelected, 2]) / 100000;

            return m_iLCFDataYs1[0];
        }

        /// <summary>
        /// 通道2--低切滤波函数（浮点型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public float LowCutFilterCh2(float fdatain)
        {
            return fdatain;
        }

        /// <summary>
        /// 通道2--低切滤波函数（长整型）
        /// </summary>
        /// <param name="idatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public long LowCutFilterCh2(long idatain)
        {
            m_iLCFDataXs2[2] = m_iLCFDataXs2[1];
            m_iLCFDataXs2[1] = m_iLCFDataXs2[0];
            m_iLCFDataXs2[0] = idatain;

            m_iLCFDataYs2[2] = m_iLCFDataYs2[1];
            m_iLCFDataYs2[1] = m_iLCFDataYs2[0];
            m_iLCFDataYs2[0] = (m_iLCFDataXs2[0] * m_iLCFCofA[this.m_iLCFSelected, 0] + m_iLCFDataXs2[1] * m_iLCFCofA[this.m_iLCFSelected, 1] + m_iLCFDataXs2[2] * m_iLCFCofA[this.m_iLCFSelected, 2]
                - m_iLCFDataYs2[1] * m_iLCFCofB[this.m_iLCFSelected, 1] - m_iLCFDataYs2[2] * m_iLCFCofB[this.m_iLCFSelected, 2]) / 100000;

            return m_iLCFDataYs2[0];
        }

        /*******************************************************/
        
        //高切滤波--频率表
        public string[] m_strHCFValue = new string[12] { "NONE", "15Hz", "30Hz", "60Hz", "100Hz", "150Hz", "200Hz", "250Hz", "300Hz", "400Hz", "500Hz", "600Hz" };

        //高切滤波--系数（对应：none、15、30、60、100、150、200、250、300、400、500、600）
        private Int32[,] m_iHCFCofA = new Int32[12, 3] { 
            {100000, 0, 0}, { 14, 27, 14 }, { 54, 107, 54 }, { 208, 416, 208 },
            {554, 1109, 554}, {1186, 2372, 1186 }, {2008,4017,2008 }, {2995, 5991, 2995 }, 
            {4125, 8251, 4125 }, { 6746, 13491, 6746}, {9763, 19526, 9763}, {13111, 26221, 13111 } };

        private Int32[,] m_iHCFCofB = new Int32[12, 3] { 
            {100000, 0, 0 }, {100000, -196668, 96723 }, { 100000, -193338, 93553 }, { 100000, -186689, 87521 },
            {100000, -177863, 80080 }, {100000,-166912, 71663 }, {100000, -156102, 64135 }, {100000,-145424, 57406 }, 
            {100000,-134897,51398 }, {100000, -114298, 41280 }, {100000, -94281, 33333}, {100000, -74779, 27221 } };

        //高切滤波--参数（外部选择的频率索引）
        public int m_iHCFSelected = 0;
        public string m_strHCFSelected
        {
            get
            {
                return m_strHCFValue[m_iHCFSelected];
            }
            set
            {
                if (int.TryParse(value, out int tmp))
                {
                    value += "Hz";
                }
                for (int i = 0; i < m_strHCFValue.Length; i++)
                {
                    if (value.ToLower() == m_strHCFValue[i].ToLower())
                    {
                        m_iHCFSelected = i;
                        return;
                    }
                }
            }
        }
        
        //高切滤波--参数（内部用）
        private long[] m_iHCFDataXs1 = new long[3];
        private long[] m_iHCFDataYs1 = new long[3];
        private long[] m_iHCFDataXs2 = new long[3];
        private long[] m_iHCFDataYs2 = new long[3];

        /// <summary>
        /// 通道1--高切滤波函数（浮点型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public float HighCutFilterCh1(float fdatain)
        {
            return fdatain;
        }

        /// <summary>
        /// 通道1--高切滤波函数（长整型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public long HighCutFilterCh1(long idatain)
        {
            m_iHCFDataXs1[2] = m_iHCFDataXs1[1];
            m_iHCFDataXs1[1] = m_iHCFDataXs1[0];
            m_iHCFDataXs1[0] = idatain;

            m_iHCFDataYs1[2] = m_iHCFDataYs1[1];
            m_iHCFDataYs1[1] = m_iHCFDataYs1[0];
            m_iHCFDataYs1[0] = (m_iHCFDataXs1[0] * m_iHCFCofA[this.m_iHCFSelected, 0] + m_iHCFDataXs1[1] * m_iHCFCofA[this.m_iHCFSelected, 1] + m_iHCFDataXs1[2] * m_iHCFCofA[this.m_iHCFSelected, 2]
                - m_iHCFDataYs1[1] * m_iHCFCofB[this.m_iHCFSelected, 1] - m_iHCFDataYs1[2] * m_iHCFCofB[this.m_iHCFSelected, 2]) / 100000;

            return m_iHCFDataYs1[0];
        }

        /// <summary>
        /// 通道2--高切滤波函数（浮点型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public float HighCutFilterCh2(float fdatain)
        {
            return fdatain;
        }

        /// <summary>
        /// 通道2--高切滤波函数（长整型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public long HighCutFilterCh2(long idatain)
        {
            m_iHCFDataXs2[2] = m_iHCFDataXs2[1];
            m_iHCFDataXs2[1] = m_iHCFDataXs2[0];
            m_iHCFDataXs2[0] = idatain;

            m_iHCFDataYs2[2] = m_iHCFDataYs2[1];
            m_iHCFDataYs2[1] = m_iHCFDataYs2[0];
            m_iHCFDataYs2[0] = (m_iHCFDataXs2[0] * m_iHCFCofA[this.m_iHCFSelected, 0] + m_iHCFDataXs2[1] * m_iHCFCofA[this.m_iHCFSelected, 1] + m_iHCFDataXs2[2] * m_iHCFCofA[this.m_iHCFSelected, 2]
                - m_iHCFDataYs2[1] * m_iHCFCofB[this.m_iHCFSelected, 1] - m_iHCFDataYs2[2] * m_iHCFCofB[this.m_iHCFSelected, 2]) / 100000;

            return m_iHCFDataYs2[0];
        }

        /*******************************************************/

        //带阻滤波--频率表（注：带阻滤波，这里指陷波）
        public string[] m_strNotchValue = new string[3] { "NONE", "50Hz", "60Hz" };

        //带阻滤波--系数（对应：none、50Hz、60Hz）
        private Int32[,] m_iNotchCofA = new Int32[3, 3] { { 100000, 0, 0 }, { 99843, -199071, 99843 }, { 99843, -198800, 99843 } };
        private Int32[,] m_iNotchCofB = new Int32[3, 3] { { 0, 0, 0 }, { 100000, -199071, 99686 }, { 100000, -198800, 99686 } };

        //带阻滤波--参数（外部选择的频率索引）
        public int m_iNotchSelected = 0;
        public string m_strNotchSelected 
        {
            get
            {
                return m_strNotchValue[m_iNotchSelected];
            }
            set
            {
                if (int.TryParse(value, out int tmp))
                {
                    value += "Hz";
                }
                for (int i = 0; i < m_strNotchValue.Length; i++)
                {
                    if (value.ToLower() == m_strNotchValue[i].ToLower())
                    {
                        m_iNotchSelected = i;
                        return;
                    }
                }
            }
        }

        //带阻滤波--参数（内部用）
        private long[] m_iNotchDataXs1 = new long[3];
        private long[] m_iNotchDataYs1 = new long[3];
        private long[] m_iNotchDataXs2 = new long[3];
        private long[] m_iNotchDataYs2 = new long[3];
        
        /// <summary>
        /// 通道1--50Hz陷波滤波器（浮点型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public float NotchFilterCh1(float fdatain)
        {
            return fdatain;
        }

        /// <summary>
        /// 通道1--50Hz陷波滤波器（长整型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public long NotchFilterCh1(long idatain)
        {
            m_iNotchDataXs1[2] = m_iNotchDataXs1[1];
            m_iNotchDataXs1[1] = m_iNotchDataXs1[0];
            m_iNotchDataXs1[0] = idatain;

            m_iNotchDataYs1[2] = m_iNotchDataYs1[1];
            m_iNotchDataYs1[1] = m_iNotchDataYs1[0];
            m_iNotchDataYs1[0] = (m_iNotchDataXs1[0] * m_iNotchCofA[this.m_iNotchSelected, 0] + m_iNotchDataXs1[1] * m_iNotchCofA[m_iNotchSelected, 1] + m_iNotchDataXs1[2] * m_iNotchCofA[m_iNotchSelected, 2]
                - m_iNotchDataYs1[1] * m_iNotchCofB[m_iNotchSelected, 1] - m_iNotchDataYs1[2] * m_iNotchCofB[m_iNotchSelected, 2]) / 100000;

            return m_iNotchDataYs1[0];
        }

        /// <summary>
        /// 通道2--50Hz陷波滤波器（浮点型）
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public float NotchFilterCh2(float fdatain)
        {
            return fdatain;
        }

        /// <summary>
        /// 通道2--50Hz陷波滤波器（长整型)
        /// </summary>
        /// <param name="fdatain">输入数据</param>
        /// <returns>滤波结果</returns>
        public long NotchFilterCh2(long idatain)
        {
            m_iNotchDataXs2[2] = m_iNotchDataXs2[1];
            m_iNotchDataXs2[1] = m_iNotchDataXs2[0];
            m_iNotchDataXs2[0] = idatain;

            m_iNotchDataYs2[2] = m_iNotchDataYs2[1];
            m_iNotchDataYs2[1] = m_iNotchDataYs2[0];
            m_iNotchDataYs2[0] = (m_iNotchDataXs2[0] * m_iNotchCofA[this.m_iNotchSelected, 0] + m_iNotchDataXs2[1] * m_iNotchCofA[this.m_iNotchSelected, 1] + m_iNotchDataXs2[2] * m_iNotchCofA[this.m_iNotchSelected, 2]
                - m_iNotchDataYs2[1] * m_iNotchCofB[this.m_iNotchSelected, 1] - m_iNotchDataYs2[2] * m_iNotchCofB[this.m_iNotchSelected, 2]) / 100000;

            return m_iNotchDataYs2[0];
        }

        #endregion


        #region 20200728 20210304 20210306 20210307 20210308 20210309

        //陷波参数
        public float[] IIR_A_50_Notch = new float[] { 0.9933829036816f, -1.980641271429f, 0.9933829036816f }; //numerator 分子
        public float[] IIR_B_50_Notch = new float[] { 1, -1.980641271429f, 0.9867658073632f };                //denominator 分母

        public float[] IIR_A_100_Notch = new float[] { 0.9868505798785f, -1.949401623311f, 0.9868505798785f }; //numerator 分子
        public float[] IIR_B_100_Notch = new float[] { 1, -1.949401623311f, 0.973701159757f };                //denominator 分母

        public float[] IIR_A_150_Notch = new float[] { 0.9881504481767f, -1.921695545269f, 0.9881504481767f }; //numerator 分子
        public float[] IIR_B_150_Notch = new float[] { 1, -1.921695545269f, 0.9763008963533f };               //denominator 分母

        public float[] IIR_A_200_Notch = new float[] { 0.9740251373157f, -1.852705907759f, 0.9740251373157f }; //numerator 分子
        public float[] IIR_B_200_Notch = new float[] { 1, -1.852705907759f, 0.9480502746315f };               //denominator 分母

        //陷波函数（涉及前两个点，若要单点计算，w01 无所谓，w02 要设为全局，w03 要设为全局）
        public void NotchFilter(float[] IIR_A, float[] IIR_B, float[] data, int dataLen)
        {
            //校验
            if (data == null)
                return;
            if (data.Length == 0)
                return;
            if (dataLen <= 0)
                return;
            if (dataLen > data.Length)
                dataLen = data.Length;

            //开始
            float w01 = 0;  //当前点参数
            float w02 = 0;  //上个点参数
            float w03 = 0;  //上上个点参数

            for (int i = 0; i < dataLen; i++)
            {
                w01 = IIR_B[0] * data[i] - IIR_B[1] * w02 - IIR_B[2] * w03;

                data[i] = IIR_A[0] * w01 + IIR_A[1] * w02 + IIR_A[2] * w03;

                w03 = w02;
                w02 = w01;
            }
        }

        /****************************************************************************************/

        //1Hz高通参数
        public float gain1_1 = 0.9985935892548f;
        public float[] IIR_A1_1 = new float[] { 1, -1.999997697884f, 1 };
        public float[] IIR_B1_1 = new float[] { 1, -1.997170248312f, 0.9972018098287f };

        public float gain2_1 = 0.9961795425917f;
        public float[] IIR_A2_1 = new float[] { 1, -1.999998766299f, 1 };
        public float[] IIR_B2_1 = new float[] { 1, -1.992343260243f, 0.992373681136f };

        public float gain3_1 = 0.9947910981181f;
        public float[] IIR_A3_1 = new float[] { 1, -1.999999834715f, 1 };
        public float[] IIR_B3_1 = new float[] { 1, -1.9895674562f, 0.9895967718493f };

        //10Hz高通参数
        public float gain1_10 = 0.9921663614345f;
        public float[] IIR_A1_10 = new float[] { 1, -1.999998766299f, 1 };
        public float[] IIR_B1_10 = new float[] { 1, -1.984270131979f, 0.9843940897225f };

        //35Hz低通参数
        public float gain1_35 = 0.006172596621834f;
        public float[] IIR_A1_35 = new float[] { 1, -1.330529785569f, 1 };
        public float[] IIR_B1_35 = new float[] { 1, -1.947420683f, 0.951553052584f };

        public float gain2_35 = 0.001859119336608f;
        public float[] IIR_A2_35 = new float[] { 1, 0.1580171471185f, 1 };
        public float[] IIR_B2_35 = new float[] { 1, -1.88243329074f, 0.8864453021472f };

        //45Hz低通参数
        public float gain1_45 = 0.008999646772611f;
        public float[] IIR_A1_45 = new float[] { 1, -1.330529785569f, 1 };
        public float[] IIR_B1_45 = new float[] { 1, -1.860311267615f, 0.869508156471f };

        //100Hz低通参数
        public float gain1_100 = 0.1284476366657f;
        public float[] IIR_A1_100 = new float[] { 1, -0.9780833528217f, 1 };               //-1.895269075929
        public float[] IIR_B1_100 = new float[] { 1, -1.931391733092f, 0.9448441727743f };

        public float gain2_100 = 0.07112729145912f;
        public float[] IIR_A2_100 = new float[] { 1, -1.808902552582f, 1 };
        public float[] IIR_B2_100 = new float[] { 1, -1.834153206991f, 0.8477454508305f };

        public float gain3_100 = 0.01286828087258f;
        public float[] IIR_A3_100 = new float[] { 1, -0.9101842985906f, 1 };
        public float[] IIR_B3_100 = new float[] { 1, -1.771384952755f, 0.7854090073f };

        //滤波函数（涉及前两个点，若要单点计算，w01 无所谓，w02 要设为全局，w03 要设为全局）
        public void PassFilter(float gain, float[] IIR_A, float[] IIR_B, float[] data, int dataLen)
        {
            //校验
            if (data == null)
                return;
            if (data.Length == 0)
                return;
            if (dataLen <= 0)
                return;
            if (dataLen > data.Length)
                dataLen = data.Length;

            //开始
            float w01 = 0;  //当前点参数
            float w02 = 0;  //上个点参数
            float w03 = 0;  //上上个点参数

            for (int i = 0; i < dataLen; i++)
            {
                float tmp = data[i] * gain;

                w01 = tmp - IIR_B[1] * w02 - IIR_B[2] * w03;

                data[i] = IIR_A[0] * w01 + IIR_A[1] * w02 + IIR_A[2] * w03;

                w03 = w02;
                w02 = w01;
            }
        }

        /****************************************************************************************/

        //平滑参数
        public static int SmoothStep = 20;

        //平滑函数（涉及前后点，无法进行动态单点计算）
        public void Smoothing(int step, float[] data, int dataLen)
        {
            //校验
            if (step <= 1)
                return;

            //校验
            if (data == null)
                return;
            if (data.Length == 0)
                return;
            if (dataLen <= 0)
                return;
            if (dataLen > data.Length)
                dataLen = data.Length;

            //开始
            float[] tmp = new float[data.Length];
            data.CopyTo(tmp, 0);

            for (int i = step; i <= dataLen - step; i++)  //todo 好像应该是 int i = step - 1;
            {
                data[i] = 0;
                for (int j = 1 - step; j <= step - 1; j++)
                {
                    data[i] = data[i] + tmp[i + j];
                }
                data[i] = data[i] / (2 * step - 1);
            }
        }

        /****************************************************************************************/

        //零点漂移（与点数密切相关，不涉及后续点）
        public void ZeroDriftSuppre(float[] data, int dataLen)
        {
            //校验
            if (data == null)
                return;
            if (data.Length == 0)
                return;
            if (dataLen <= 0)
                return;
            if (dataLen > data.Length)
                dataLen = data.Length;

            //开始
            float sum = 0;
            for (int i = 0; i < dataLen; i++)
            {
                sum += data[i];
            }

            float average = sum / dataLen;

            for (int i = 0; i < dataLen; i++)
            {
                data[i] -= average;
            }
        }

        /****************************************************************************************/

        //幅度矫正，又单位转换（只涉及当前点）//暂不用
        public void AmplitudeCorrection(float ampFactor, float[] data, int dataLen)
        {
            //校验
            if (data == null)
                return;
            if (data.Length == 0)
                return;
            if (dataLen <= 0)
                return;
            if (dataLen > data.Length)
                dataLen = data.Length;

            //开始
            for (int i = 0; i < dataLen; i++)
            {
                data[i] *= ampFactor * 0.01f;  //幅度校正，又单位转换（0.01uV --> uV）
            }
        }

        /****************************************************************************************/

        //相反数（只涉及当前点）//暂不用
        public void OppositeNumber(float[] data, int dataLen)
        {
            //校验
            if (data == null)
                return;
            if (data.Length == 0)
                return;
            if (dataLen <= 0)
                return;
            if (dataLen > data.Length)
                dataLen = data.Length;

            //开始
            for (int i = 0; i < dataLen; i++)
            {
                data[i] = 0 - data[i];
            }
        }

        #endregion


    }
}
